#include "quaternion.h"

namespace visualization
{

	Quaternion& Quaternion::operator *= (const Quaternion &q)
	{
		double tempS = s;
		s = tempS * q.S() - Vector::Dot(v, q.Vec());
		v = q.Vec() * tempS + v * q.S() + Vector::Cross(v, q.Vec());
		return *this;
	}

	/*
	* Build a rotation matrix, given a quaternion rotation.
	*/
	void Quaternion::BuildMatrix(double(&m)[16])
	{
		double x2 = v.x * v.x, y2 = v.y * v.y, z2 = v.z * v.z;
		double xy = v.x * v.y, yz = v.y * v.z, zx = v.z * v.x;
		double sx = s * v.x, sy = s * v.y, sz = s * v.z;

		m[0] = 1.0 - 2.0 * (y2 + z2);
		m[1] = 2.0 * (xy - sz);
		m[2] = 2.0 * (zx + sy);
		m[3] = 0.0;

		m[4] = 2.0 * (xy + sz);
		m[5] = 1.0 - 2.0 * (z2 + x2);
		m[6] = 2.0 * (yz - sx);
		m[7] = 0.0;

		m[8] = 2.0 * (zx - sy);
		m[9] = 2.0 * (yz + sx);
		m[10] = 1.0- 2.0 * (x2 + y2);
		m[11] = 0.0;

		m[12] = 0.0;
		m[13] = 0.0;
		m[14] = 0.0;
		m[15] = 1.0;
	}

	const Quaternion operator * (const Quaternion &q1, const Quaternion &q2)
	{
		return Quaternion(
			q2.Vec() * q1.S() + q1.Vec() * q2.S()
			+ Vector::Cross(q1.Vec(), q2.Vec()),
			q1.S()*q2.S() - Vector::Dot(q1.Vec(), q2.Vec()));
	}
}

